#include "haar.h"

#include <string.h>
#include <math.h>
#include <stdlib.h>
#include <queue>

namespace oe {
const decimal Haar::kInvSqrt2 = 1.f / sqrtf(2.f);

Haar::Haar(int dim, int m) : dim_(dim), m_(m) {
  size_ = dim_ * dim_;
  array_ = new decimal[dim];
}

Haar::~Haar() {
  delete[] array_;
  array_ = NULL;
  dim_ = 0;
}

void Haar::RGB2YIQ(decimal *A, decimal *B, decimal *C) {
  for (int i = 0; i < size_; ++i) {
    decimal Y = 0.299 * A[i] + 0.587 * B[i] + 0.114 * C[i];
    decimal I = 0.596 * A[i] - 0.275 * B[i] - 0.321 * C[i];
    decimal Q = 0.212 * A[i] - 0.523 * B[i] + 0.311 * C[i];
    A[i] = Y; 
    B[i] = I;
    C[i] = Q;
  }
}

void Haar::Transpose(decimal *A) {
  decimal *r = A;
  for (int i = 0; i < dim_; ++i) {
    decimal *c = r + dim_ + i;
    for (int j = i + 1; j < dim_; ++j) {
      decimal tmp = r[j];
      r[j] = c[0];
      c[0] = tmp;
      c += dim_;
    }
    r += dim_;
  }
}

#if 1
void Haar::Transform(decimal *A) {
  Haar1Pass(A);
  Transpose(A); 

  Haar1Pass(A);
  Transpose(A);
}
#else
#define NUM_PIXELS 128
#define NUM_PIXELS_SQUARED (NUM_PIXELS * NUM_PIXELS)
void Haar::Transform(decimal *A) {
  int i;
  decimal t[NUM_PIXELS >> 1];

  // scale by 1/sqrt(128) = 0.08838834764831843:
  /*
  for (i = 0; i < NUM_PIXELS_SQUARED; i++)
    a[i] *= 0.08838834764831843;
  */

  // Decompose rows:
  for (i = 0; i < NUM_PIXELS_SQUARED; i += NUM_PIXELS) {
    int h, h1;
    decimal C = 1;

    for (h = NUM_PIXELS; h > 1; h = h1) {
      int j1, j2, k;

      h1 = h >> 1;		// h = 2*h1
      C *= 0.7071;		// 1/sqrt(2)
      for (k = 0, j1 = j2 = i; k < h1; k++, j1++, j2 += 2) {
        int j21 = j2+1;

        t[k]  = (A[j2] - A[j21]) * C;
        A[j1] = (A[j2] + A[j21]);
      }
      // Write back subtraction results:
      memcpy(A+i+h1, t, h1*sizeof(A[0]));
    }
    // Fix first element of each row:
    A[i] *= C;	// C = 1/sqrt(NUM_PIXELS)
  }

  // scale by 1/sqrt(128) = 0.08838834764831843:
  /*
  for (i = 0; i < NUM_PIXELS_SQUARED; i++)
    a[i] *= 0.08838834764831843;
  */

  // Decompose columns:
  for (i = 0; i < NUM_PIXELS; i++) {
    decimal C = 1;
    int h, h1;

    for (h = NUM_PIXELS; h > 1; h = h1) {
      int j1, j2, k;

      h1 = h >> 1;
      C *= 0.7071;		// 1/sqrt(2) = 0.7071
      for (k = 0, j1 = j2 = i; k < h1;
	   k++, j1 += NUM_PIXELS, j2 += 2*NUM_PIXELS) {
        int j21 = j2+NUM_PIXELS;

        t[k]  = (A[j2] - A[j21]) * C;
        A[j1] = (A[j2] + A[j21]);
      }
      // Write back subtraction results:
      for (k = 0, j1 = i+h1*NUM_PIXELS; k < h1; k++, j1 += NUM_PIXELS)
        A[j1]=t[k];
    }
    // Fix first element of each column:
    A[i] *= C;
  }
}
#endif

void Haar::Haar1Pass(decimal *A) {
  decimal *ptr = A;
  for (int i = 0; i < dim_; ++i) {
    Haar1D(ptr);
    ptr += dim_;
  }
}

void Haar::Haar1D(decimal *A) {
  // row <- row / sqrt(h)    
  int n = dim_;
  for (int i = 0; i < n; ++i) {
    //A[i] *= norm_factor_;
  }
  while (n > 1) {
    int h = n >> 1;
    for (int i = 0; i < h; ++i) {
      int idx = i << 1;
      array_[i] = (A[idx] + A[idx + 1]) * kInvSqrt2;
      array_[h + i] = (A[idx] - A[idx + 1]) * kInvSqrt2;
    }
    memcpy(A, array_, n * sizeof(decimal));
    n = h;
  }
}

void Haar::GetCoefs(const decimal *A, int *sig, double *avg) {  
  *avg = A[0];

  std::priority_queue<Coefficient> q;

  int i = 1;
  for (; i <= m_; ++i) {
    Coefficient c;
    c.idx = i;
    c.val = abs(A[i]);
    q.push(c);
  }

  for (; i < size_; ++i) {
    Coefficient c;
    c.val = abs(A[i]);
    if (c.val > q.top().val) {
      q.pop();
      c.idx = i;
      q.push(c);
    }
  }

  i = 0;
  while (!q.empty()) {
    int t;
    Coefficient c = q.top();
    t = (A[c.idx] <= 0);
    sig[i++] = (c.idx - t) ^ -t;
    q.pop();
  };
}

} // oe
